/**************************************************************************************************
*                                                                                                 *
* This file is part of HPIPM.                                                                     *
*                                                                                                 *
* HPIPM -- High-Performance Interior Point Method.                                                *
* Copyright (C) 2019 by Gianluca Frison.                                                          *
* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
* All rights reserved.                                                                            *
*                                                                                                 *
* The 2-Clause BSD License                                                                        *
*                                                                                                 *
* Redistribution and use in source and binary forms, with or without                              *
* modification, are permitted provided that the following conditions are met:                     *
*                                                                                                 *
* 1. Redistributions of source code must retain the above copyright notice, this                  *
*    list of conditions and the following disclaimer.                                             *
* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
*    this list of conditions and the following disclaimer in the documentation                    *
*    and/or other materials provided with the distribution.                                       *
*                                                                                                 *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
*                                                                                                 *
* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
*                                                                                                 *
**************************************************************************************************/

/**
*
* Basic example of solving an OCP QP with data provided in the format generated by the routine
* d_dense_qp_codegen(...);
*
**/

#include <stdlib.h>
#include <stdio.h>

#include <blasfeo_d_aux_ext_dep.h>

#include <hpipm_d_dense_qp_ipm.h>
#include <hpipm_d_dense_qp_dim.h>
#include <hpipm_d_dense_qp.h>
#include <hpipm_d_dense_qp_sol.h>
#include <hpipm_d_dense_qp_utils.h>
#include <hpipm_timing.h>



// qp data as global data
extern int nv;
extern int ne;
extern int nb;
extern int ng;
extern int nsb;
extern int nsg;
extern int ns;
extern double *H;
extern double *g;
extern double *A;
extern double *b;
extern int *idxb;
extern double *lb;
extern double *ub;
extern double *lb_mask;
extern double *ub_mask;
extern double *C;
extern double *lg;
extern double *ug;
extern double *lg_mask;
extern double *ug_mask;
extern double *Zl;
extern double *Zu;
extern double *zl;
extern double *zu;
extern double *lls;
extern double *lus;
extern double *lls_mask;
extern double *lus_mask;
extern int *idxs_rev;
// arg
extern int mode;
extern int iter_max;
extern double alpha_min;
extern double mu0;
extern double tol_stat;
extern double tol_eq;
extern double tol_ineq;
extern double tol_comp;
extern double reg_prim;
extern double reg_dual;
extern int warm_start;
extern int pred_corr;
extern int split_step;



// main
int main()
	{

	int ii, jj;

	int hpipm_status;

	int rep, nrep=10;

	hpipm_timer timer;

/************************************************
* dense qp dim
************************************************/

	hpipm_size_t dim_size = d_dense_qp_dim_memsize();
	void *dim_mem = malloc(dim_size);

	struct d_dense_qp_dim dim;
	d_dense_qp_dim_create(&dim, dim_mem);

	//d_dense_qp_dim_set_all(nv, ne, nb, ng, nsb, nsg, &dim);
	d_dense_qp_dim_set_nv(nv, &dim);
	d_dense_qp_dim_set_ne(ne, &dim);
	d_dense_qp_dim_set_nb(nb, &dim);
	d_dense_qp_dim_set_ng(ng, &dim);
	d_dense_qp_dim_set_nsb(nsb, &dim);
	d_dense_qp_dim_set_nsg(nsg, &dim);

//	d_dense_qp_dim_codegen("examples/c/data/test_d_dense_data.c", "w", &dim);

/************************************************
* dense qp
************************************************/

	hpipm_size_t qp_size = d_dense_qp_memsize(&dim);
	void *qp_mem = malloc(qp_size);

	struct d_dense_qp qp;
	d_dense_qp_create(&dim, &qp, qp_mem);

	//d_dense_qp_set_all(...., &qp);
	d_dense_qp_set_H(H, &qp);
	d_dense_qp_set_g(g, &qp);
	d_dense_qp_set_A(A, &qp);
	d_dense_qp_set_b(b, &qp);
	d_dense_qp_set_idxb(idxb, &qp);
	d_dense_qp_set_lb(lb, &qp);
	d_dense_qp_set_ub(ub, &qp);
	d_dense_qp_set_lb_mask(lb_mask, &qp);
	d_dense_qp_set_ub_mask(ub_mask, &qp);
	d_dense_qp_set_C(C, &qp);
	d_dense_qp_set_lg(lg, &qp);
	d_dense_qp_set_ug(ug, &qp);
	d_dense_qp_set_lg_mask(lg_mask, &qp);
	d_dense_qp_set_ug_mask(ug_mask, &qp);
	d_dense_qp_set_Zl(Zl, &qp);
	d_dense_qp_set_Zu(Zu, &qp);
	d_dense_qp_set_zl(zl, &qp);
	d_dense_qp_set_zu(zu, &qp);
	d_dense_qp_set_lls(lls, &qp);
	d_dense_qp_set_lus(lus, &qp);
	d_dense_qp_set_lls_mask(lls_mask, &qp);
	d_dense_qp_set_lus_mask(lus_mask, &qp);
	d_dense_qp_set_idxs_rev(idxs_rev, &qp);

//	d_dense_qp_codegen("examples/c/data/test_d_dense_data.c", "a", &dim, &qp);

/************************************************
* dense qp sol
************************************************/

	hpipm_size_t qp_sol_size = d_dense_qp_sol_memsize(&dim);
	void *qp_sol_mem = malloc(qp_sol_size);

	struct d_dense_qp_sol qp_sol;
	d_dense_qp_sol_create(&dim, &qp_sol, qp_sol_mem);

/************************************************
* ipm arg
************************************************/

	hpipm_size_t ipm_arg_size = d_dense_qp_ipm_arg_memsize(&dim);
	void *ipm_arg_mem = malloc(ipm_arg_size);

	struct d_dense_qp_ipm_arg arg;
	d_dense_qp_ipm_arg_create(&dim, &arg, ipm_arg_mem);

	d_dense_qp_ipm_arg_set_default(mode, &arg);

	d_dense_qp_ipm_arg_set_mu0(&mu0, &arg);
	d_dense_qp_ipm_arg_set_iter_max(&iter_max, &arg);
	d_dense_qp_ipm_arg_set_alpha_min(&alpha_min, &arg);
	d_dense_qp_ipm_arg_set_mu0(&mu0, &arg);
	d_dense_qp_ipm_arg_set_tol_stat(&tol_stat, &arg);
	d_dense_qp_ipm_arg_set_tol_eq(&tol_eq, &arg);
	d_dense_qp_ipm_arg_set_tol_ineq(&tol_ineq, &arg);
	d_dense_qp_ipm_arg_set_tol_comp(&tol_comp, &arg);
	d_dense_qp_ipm_arg_set_reg_prim(&reg_prim, &arg);
	d_dense_qp_ipm_arg_set_reg_dual(&reg_dual, &arg);
	d_dense_qp_ipm_arg_set_warm_start(&warm_start, &arg);
	d_dense_qp_ipm_arg_set_pred_corr(&pred_corr, &arg);
	d_dense_qp_ipm_arg_set_split_step(&split_step, &arg);

//	d_dense_qp_ipm_arg_codegen("examples/c/data/test_d_dense_data.c", "a", &dim, &arg);

/************************************************
* ipm workspace
************************************************/

	hpipm_size_t ipm_size = d_dense_qp_ipm_ws_memsize(&dim, &arg);
	void *ipm_mem = malloc(ipm_size);

	struct d_dense_qp_ipm_ws workspace;
	d_dense_qp_ipm_ws_create(&dim, &arg, &workspace, ipm_mem);

/************************************************
* ipm solver
************************************************/

	hpipm_tic(&timer);

	for(rep=0; rep<nrep; rep++)
		{
		// call solver
		d_dense_qp_ipm_solve(&qp, &qp_sol, &arg, &workspace);
		d_dense_qp_ipm_get_status(&workspace, &hpipm_status);
		}

	double time_ipm = hpipm_toc(&timer) / nrep;

/************************************************
* print solution info
************************************************/

    printf("\nHPIPM returned with flag %i.\n", hpipm_status);
    if(hpipm_status == 0)
		{
        printf("\n -> QP solved!\n");
		}
	else if(hpipm_status==1)
		{
        printf("\n -> Solver failed! Maximum number of iterations reached\n");
		}
	else if(hpipm_status==2)
		{
        printf("\n -> Solver failed! Minimum step lenght reached\n");
		}
	else if(hpipm_status==3)
		{
        printf("\n -> Solver failed! NaN in computations\n");
		}
	else
		{
        printf("\n -> Solver failed! Unknown return flag\n");
		}
    printf("\nAverage solution time over %i runs: %e [s]\n", nrep, time_ipm);
	printf("\n\n");

/************************************************
* extract and print solution
************************************************/

	// v

	double *v = malloc(nv*sizeof(double));

	printf("\nv = \n");
	d_dense_qp_sol_get_v(&qp_sol, v);
	d_print_mat(1, nv, v, 1);

//	d_dense_qp_sol_print(&dim, &qp_sol);

/************************************************
* print ipm statistics
************************************************/

	int iter; d_dense_qp_ipm_get_iter(&workspace, &iter);
	double res_stat; d_dense_qp_ipm_get_max_res_stat(&workspace, &res_stat);
	double res_eq; d_dense_qp_ipm_get_max_res_eq(&workspace, &res_eq);
	double res_ineq; d_dense_qp_ipm_get_max_res_ineq(&workspace, &res_ineq);
	double res_comp; d_dense_qp_ipm_get_max_res_comp(&workspace, &res_comp);
	double *stat; d_dense_qp_ipm_get_stat(&workspace, &stat);
	int stat_m; d_dense_qp_ipm_get_stat_m(&workspace, &stat_m);

	printf("\nipm return = %d\n", hpipm_status);
	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);

	printf("\nipm iter = %d\n", iter);
	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);

	printf("\ndense ipm time = %e [s]\n\n", time_ipm);

/************************************************
* free memory and return
************************************************/

    free(dim_mem);
    free(qp_mem);
	free(qp_sol_mem);
	free(ipm_arg_mem);
	free(ipm_mem);

	free(v);

	return 0;

	}


